﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Text;
using System.Threading.Tasks;

namespace g4.intersection
{
    public class IntrSegment3Segment3
    {
        private Segment3d _line1;
        private Segment3d _line2;
        public IntersectionResult Result { get; set; } = IntersectionResult.NotComputed;
        public IntersectionType Type { get; set; } = IntersectionType.Empty;
        public Vector3d IntersectDot { get; set; }

        public IntrSegment3Segment3(Segment3d line1, Segment3d line2)
        {
            _line1 = line1;
            _line2 = line2;
        }

        public IntrSegment3Segment3 Compute()
        {
            Find();
            return this;
        }

        public bool Find()
        {
            if (Result != IntersectionResult.NotComputed)
                return (Result == IntersectionResult.Intersects);

            Vector3d a = _line1.P0;
            Vector3d b = _line1.P1;
            Vector3d c = _line2.P0;
            Vector3d d = _line2.P1;

            Vector3d ab = b - a;
            Vector3d ca = a - c;
            Vector3d cd = d - c;

            Vector3d v1 = Vector3d.Cross(ca, cd);

            if (Math.Abs(Vector3d.Dot(v1, ab)) > 1e-6)
            {
                Result = IntersectionResult.NoIntersection;
                // 不共面
                return false;
            }

            if (Vector3d.Cross(ab, cd).sqrMagnitude <= 1e-6)
            {
                Result = IntersectionResult.NoIntersection;
                // 平行
                return false;
            }

            Vector3d ad = d - a;
            Vector3d cb = b - c;

            // 快速排斥
            if (Math.Min(a.x, b.x) > Math.Max(c.x, d.x) || Math.Max(a.x, b.x) < Math.Min(c.x, d.x) ||
                Math.Min(a.y, b.y) > Math.Max(c.y, d.y) || Math.Max(a.y, b.y) < Math.Min(c.y, d.y) ||
                Math.Min(a.z, b.z) > Math.Max(c.z, d.z) || Math.Max(a.z, b.z) < Math.Min(c.z, d.z))
            {
                Result = IntersectionResult.NoIntersection;
                return false;
            }

            // 跨立试验
            if (Vector3d.Dot(Vector3d.Cross(-ca, ab), Vector3d.Cross(ab, ad)) <= 0
                 || Vector3d.Dot(Vector3d.Cross(ca, cd), Vector3d.Cross(cd, cb)) <= 0)
            {
                Result = IntersectionResult.NoIntersection;
                return false;
            }

            Vector3d v2 = Vector3d.Cross(cd, ab);
            double ratio = Vector3d.Dot(v1, v2) / v2.sqrMagnitude;
            IntersectDot = a + ab * ratio;

            Result = IntersectionResult.Intersects;
            Type = IntersectionType.Point;
            return true;

        }
    }
}
